#include "IMUSensorVisualisation.h"

#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoUnits.h>
#include <Inventor/nodes/SoScale.h>
#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/engines/SoCalculator.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>

#include <tuple>

using namespace MMM;

IMUSensorVisualisation::IMUSensorVisualisation(IMUSensorPtr sensor, VirtualRobot::RobotPtr robot, SoSeparator* sceneSep) :
    SensorVisualisation(sceneSep),
    sensor(sensor),
    robot(robot)
{
}

std::string IMUSensorVisualisation::getType() {
    return sensor->getType();
}

int IMUSensorVisualisation::getPriority() {
    return sensor->getPriority();
}

SoSeparator* IMUSensorVisualisation::CreateCoordSystemVisualization(const Eigen::Vector3f &acceleration, float scaling, float axisSize, int nrOfBlocks)
{
    float blockSize = axisSize + 0.5f;
    float blockWidth = 0.1f;
    if (axisSize > 10.0f)
    {
        blockSize += axisSize / 10.0f;
        blockWidth += axisSize / 10.0f;
    }

    SoSeparator* result = new SoSeparator();
    result->ref();
    SoUnits *u = new SoUnits();
    u->units = SoUnits::MILLIMETERS;
    result->addChild(u);

    SbMatrix m;
    m.makeIdentity();
    SoMatrixTransform *mtr = new SoMatrixTransform();
    mtr->matrix.setValue(m);
    result->addChild(mtr);

    for (int i=0; i < 3; i++)
    {
        float axisBlockTranslation;
        if (nrOfBlocks != 0)
        {
            axisBlockTranslation = acceleration[i] / nrOfBlocks;
        } else
            axisBlockTranslation = acceleration[i] / 10.0f;

        SoSeparator *tmp1 = new SoSeparator();
        SoTransform *t = new SoTransform();
        SoMaterial *m = new SoMaterial();
        if (i == 0)
        {
            m->diffuseColor.setValue(1.0f, 0, 0);
            t->translation.setValue((acceleration[i] / 2.0f + axisSize / 2.0f) * scaling, 0, 0);
        } else if (i == 1)
        {
            m->diffuseColor.setValue(0, 1.0f, 0);
            t->translation.setValue(0,(acceleration[i] / 2.0f + axisSize / 2.0f) * scaling, 0);
        } else
        {
            m->diffuseColor.setValue(0, 0, 1.0f);
            t->translation.setValue(0, 0, (acceleration[i] / 2.0f + axisSize / 2.0f) * scaling);
        }

        tmp1->addChild(m);
        tmp1->addChild(t);
        SoCube *c = new SoCube();
        SoCube *c2 = new SoCube();
        SoTransform *t2 = new SoTransform();
        if (i == 0)
        {
            c->width = acceleration[i] * scaling;
            c->height = axisSize * scaling;
            c->depth = axisSize * scaling;
            c2->width = blockWidth * scaling;
            c2->height = blockSize * scaling;
            c2->depth = blockSize * scaling;
            t2->translation.setValue(axisBlockTranslation * scaling, 0, 0);
        } else if (i == 1)
        {
            c->height = acceleration[i] * scaling;
            c->width = axisSize * scaling;
            c->depth = axisSize * scaling;
            c2->width = blockSize * scaling;
            c2->height = blockWidth * scaling;
            c2->depth = blockSize * scaling;
            t2->translation.setValue(0, axisBlockTranslation * scaling, 0);
        } else
        {
            c->depth = acceleration[i] * scaling;
            c->height = axisSize * scaling;
            c->width = axisSize * scaling;
            c2->width = blockSize * scaling;
            c2->height = blockSize * scaling;
            c2->depth = blockWidth * scaling;
            t2->translation.setValue(0, 0, axisBlockTranslation * scaling);
        }
        tmp1->addChild(c);
        result->addChild(tmp1);

        SoSeparator *tmp2 = new SoSeparator();
        SoMaterial *m2 = new SoMaterial();
        m2->diffuseColor.setValue(1.0f, 1.0f, 1.0f);
        tmp2->addChild(m2);

        for (int j=0; j < nrOfBlocks; j++)
        {
            tmp2->addChild(t2);
            tmp2->addChild(c2);
        }

        result->addChild(tmp2);
    }

    return result;
}

void IMUSensorVisualisation::update(float timestep, float delta) {
    update(sensor->getDerivedMeasurement(timestep, delta));
}

void IMUSensorVisualisation::update(float timestep) {
    update(sensor->getDerivedMeasurement(timestep));
}

void IMUSensorVisualisation::update(IMUSensorMeasurementPtr measurement) {
    if (!measurement) return;

    VirtualRobot::RobotNodePtr segmentNode = robot->getRobotNode(sensor->getSegment());
    Eigen::Matrix4f segmentPose = segmentNode->getGlobalPose();
    Eigen::Matrix4f imuPosition = segmentPose * sensor->getOffset();
    if (!hasVisualisationSep()) {
        SoSeparator* imuSep = new SoSeparator;
        imuSep->addChild(VirtualRobot::CoinVisualizationFactory::getMatrixTransform(imuPosition));
        imuSep->addChild(IMUSensorVisualisation::CreateCoordSystemVisualization(measurement->getAcceleration()));
        setVisualisationSep(imuSep);
    } else {
        SoMatrixTransform* mt = (SoMatrixTransform*) getChildVisualisationSep(0);
        SbMatrix m_(reinterpret_cast<SbMat*>(imuPosition.data()));
        mt->matrix.setValue(m_);
        removeChildVisualisationSep(1);
        addChildVisualisationSep(IMUSensorVisualisation::CreateCoordSystemVisualization(measurement->getAcceleration()));
    }
}
